
/**
  ****************************(C) COPYRIGHT HCRT****************************
  * @file       jump.c/h
  * @brief      跳跃实现函数
  * @details    跳跃规划生成以及执行
  * @author     ylt
  * @date       2020-7-26
  * @version    V1.1
  * @copyright  Copyright (c) 2018-2020
  ==========================================================================
  **************************** HBUT ROBOCON 2020****************************
*/

#include <webots/robot.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>

#include "jump.h"
#include "gait.h"

float temp_time0;
float start_time_ = 0.0f; // 跳跃开始的时刻 [s]

// float prep_angle = 23;  //起跳准备时腿的角度 [度]
// float jump_angle = 35;  //起跳瞬间腿的角度 [度]
// float fall_angle = -10; //起跳瞬间腿的角度 [度]

float prep_angle = 0; // 起跳准备时腿的角度 [度]
float jump_angle = 0; // 起跳瞬间腿的角度 [度]
float fall_angle = 0; // 起跳瞬间腿的角度 [度]


float prep_angle_motoy = 20; // 起跳准备时腿的角度 [度]

// float prep_angle = 20; // 起跳准备时腿的角度 [度]
// float angle = 0;
// float last_angle = 0;
// float output_angle = 0;

// void JumpTrajSmooth(float input)
// {
//     static float pre_input;
//     static float output;

//     pre_input = input;

//     if (pre_input > input)
//         output += 1;
//     else if (pre_input < input)
//         output -= 1;
//     else
//         output = output;
//     return output:
// }
static float output_height = 18.0f;

void ExecuteJump()
{
    const float prep_time = 0.3;   // 起跳准备时间 [s]
    const float launch_time = 0.3f; // 收腿持续时间 [s]
    const float fall_time = 0.4;  // 收缩腿后持续时间恢复正常行为 [s]
    
    const float stance_height = 14.0f;  // 起跳之前腿的长度 [cm]
    const float jump_extension = 28.8f; // 空中最大伸腿长度 [cm]
    const float fall_extension = 14.0f; // 降落时的期望腿长 [cm]

    float x, y, theta, gamma;

    static float angle, last_angle, output_angle;

    float t = wb_robot_get_time() - start_time_; // 跳跃开始后的时间 [s]
                                                 // printf("t ：%f.\r\n", t);

    last_angle = output_angle;
    if (angle > last_angle)
        output_angle += 1;
    else if (angle < last_angle)
        output_angle -= 1;
    else
        output_angle = output_angle;

    // printf("Jump Complete.\r\n");

    // last_height = output_height;
    // if (height > last_height)
    //     output_height += 0.1;
    // else if (height < last_height)
    //     output_height -= 0.1;
    // else
    //     output_height = output_height;

    if (t < prep_time)
    {
        // 200ms 一个step 10ms
        // static float temp_cnt = 0;

        // float temp_height = 25.0f - (0.4f) * temp_cnt;
        // if(temp_height <= 14.0f)
        //     temp_height = 14.0f;
        // // printf("temp_height ：%f.\r\n", temp_height);
        // temp_cnt++;

        angle = prep_angle;
        x = -stance_height * sin(output_angle * PI / 180);
        y = +stance_height * cos(output_angle * PI / 180);
        //脚xy轨迹转换为两电机的角度 theta、gamma
        CartesianToThetaGamma(x, y, 1.0, &theta, &gamma);
        // 使用刚度小，阻尼大的增益
        LegGain_t gains = {15, 0.00, 8, 0.00};

        // angle = prep_angle_motoy
        // gamma = output_angle;
        // theta = gamma;
        // printf("theta ：%f. gamma ：%f. \r\n", theta, gamma);
        // 同时驱动所有腿
        CommandAllLegs(theta, gamma, gains);
    }
    else if (t >= prep_time && t < prep_time + launch_time)
    {
        angle = jump_angle;
        output_angle = -9;
        x = -jump_extension * sin(output_angle * PI / 180);
        y = +jump_extension * cos(output_angle * PI / 180);
        // printf("y ：%f.\r\n", y);
        // 脚xy轨迹转换为两电机的角度 theta、gamma
        CartesianToThetaGamma(x, y, 1.0, &theta, &gamma);
        // 使用高刚度和低阻尼执行跳转
        LegGain_t gains = {22.0, 0.00, 16.0, 0.00};
        // 同时驱动所有腿
        CommandAllLegs(theta, gamma, gains);
    }
    else if (t >= prep_time + launch_time && t < prep_time + launch_time + fall_time)
    {
        angle = fall_angle;
        x = -fall_extension * sin(output_angle * PI / 180);
        y = +fall_extension * cos(output_angle * PI / 180);
        //脚xy轨迹转换为两电机的角度 theta、gamma
        CartesianToThetaGamma(x, y, 1.0, &theta, &gamma);
        //使用低刚度和大量的阻尼来处理下降 前位置环 后速度环
        LegGain_t gains = {15, 0.00, 1.8, 0.00};
        // 同时驱动所有腿
        CommandAllLegs(theta, gamma, gains);
    }
    else
    {
        // LegGain gains = {22.0, 0.00, 8.0, 0.00};
        //  pre_prep_angle = 0;
        //  temp_prep_angle = fall_angle;
        temp_time0 = wb_robot_get_time();
        GAIT_STATE = STOP;

        printf("Jump Complete.\r\n");
    }
}

void BackSomersault()
{
    //     float state1_angle = 23;  //起跳准备时腿的角度 [度]
    //     float state2_angle = 35;  //起跳瞬间腿的角度 [度]
    //     float state3_angle = -35; //起跳瞬间腿的角度 [度]

    //     const float state1_time = 0.95f; // 起跳准备时间 [s]
    //     const float state2_time = 0.2f;  // 收腿持续时间 [s]
    //     const float state3_time = 0.8f;  //收缩腿后持续时间恢复正常行为 [s]

    //     const float state1_extension = 15.8f; // 起跳之前腿的长度 [cm]
    //     const float state2_extension = 28.8f; // 空中最大伸腿长度 [cm]
    //     const float state3_extension = 15.8f; // 降落时的期望腿长 [cm]

    //     float x, y, theta, gamma;
    //     static float angle, last_angle, output_angle;

    //     float t = wb_robot_get_time() - start_time_; // 跳跃开始后的时间 [s]

    //     last_angle = output_angle;
    //     if (angle > last_angle)
    //         output_angle += 1;
    //     else if (angle < last_angle)
    //         output_angle -= 1;
    //     else
    //         output_angle = output_angle;

    //     if (t < state1_time)
    //     {
    //         angle = state1_angle;
    //         x = -state1_extension * sin(output_angle * PI / 180);
    //         y = +state1_extension * cos(output_angle * PI / 180);
    //         //脚xy轨迹转换为两电机的角度 theta、gamma
    //         CartesianToThetaGamma(x, y, 1.0, &theta, &gamma);
    //         // 使用刚度小，阻尼大的增益
    //         LegGain_t gains = {5, 0.00, 8, 0.00};
    //         // 同时驱动所有腿
    //         SetCoupledPosition(0, theta, gamma);
    //         SetCoupledPosition(3, theta, gamma);
    //     }
    //     else if (t >= state1_time && t < state1_time + state2_time)
    //     {
    //         angle = state2_angle;
    //         x = -state2_extension * sin(output_angle * PI / 180);
    //         y = +state2_extension * cos(output_angle * PI / 180);
    //         //脚xy轨迹转换为两电机的角度 theta、gamma
    //         CartesianToThetaGamma(x, y, 1.0, &theta, &gamma);
    //         // 使用高刚度和低阻尼执行跳转
    //         LegGain_t gains = {22.0, 0.00, 16.0, 0.00};
    //         // 同时驱动所有腿
    //         CommandAllLegs(theta, gamma, gains);
    //     }
    //     else if (t >= state1_time + state2_time && t < state1_time + state2_time + state3_time)
    //     {
    //         angle = state3_angle;
    //         x = -state3_extension * sin(output_angle * PI / 180);
    //         y = +state3_extension * cos(output_angle * PI / 180);
    //         //脚xy轨迹转换为两电机的角度 theta、gamma
    //         CartesianToThetaGamma(x, y, 1.0, &theta, &gamma);
    //         //使用低刚度和大量的阻尼来处理下降 前位置环 后速度环
    //         LegGain_t gains = {15, 0.00, 1.8, 0.00};
    //         // 同时驱动所有腿
    //         SetCoupledPosition(0, theta, gamma);
    //         SetCoupledPosition(1, theta, gamma);
    //     }
    //     else
    //     {
    //         //LegGain gains = {22.0, 0.00, 8.0, 0.00};
    //         // pre_prep_angle = 0;
    //         temp_time0 = wb_robot_get_time();
    //         GAIT_STATE = STOP;
    //         printf("Jump Complete.\r\n");
    //     }
}